
import time
import wx
import vtk
import math
import numpy
import vtktudoss
try:
    import extended.BaseCalc as BaseCalc
except ImportError:
    import BaseCalc


def create(parent):    
    return InverseKinematics(parent)
    
class InverseKinematics:
    constantangle = 0.5
    
    def __init__(self, parent):
        print "InverseKinematics module loaded."        
        self.parent = parent
        self.baseCalc = BaseCalc.create(self, self)        

    def SingleGradient(self, cor, axis, tip, target):
        ToTip = tip - cor
        CorToTarget = target - cor
        ToTarget = target - tip

        movement_vector = numpy.cross(ToTip, axis)
        movement_vector = movement_vector/ numpy.linalg.norm(movement_vector)
        ToTarget = ToTarget/ numpy.linalg.norm(ToTarget)
        gradient = numpy.dot(movement_vector, ToTarget)    
        #~ controle_as = numpy.cross(CorToTarget, ToTip) #determines + or - angle
        #~ print "controle_as: ", controle_as
        #~ controle_as = controle_as/ numpy.linalg.norm(controle_as)
        #~ print "controle_as: ", controle_as
        #~ controle_hoek = numpy.dot(controle_as, axis)
        #~ if controle_hoek>0:
            #~ ch = 1
        #~ else :
            #~ ch = -1
        return gradient
        
        
        
        
    def Distance(self, a, b):
        return numpy.linalg.norm(a-b)
    
    #~ def DetermineGradients(self, start, target, OXYZ):

        #~ for i in range(0, len(stance_rates)/3):
            #~ stance_rates[0] = self.SingleGradient(c, a, X_axis, b)
            #~ stance_rates[1] = self.SingleGradient(c, a, Y_axis, b)
            #~ stance_rates[2] = self.SingleGradient(c, a, Z_axis, b)
        
        #~ for i in range(0, len(stance_rates)):
            #~ if numpy.isnan(stance_rates[i]):
                #~ stance_rates[i] = 0.0
            #~ elif stance_rates[i] < self.stance_min[i]:
                #~ stance_rates[i] = self.stance_min[i]
            #~ elif stance_rates[i] > self.stance_max[i]:
                #~ stance_rates[i] = self.stance_max[i]
                
        #~ print stance_rates[0]
        #~ print stance_rates[1]
        #~ print stance_rates[2]

        #~ for i in range(0, len(stance)):
            #~ stance[i] = stance[i] + stance_rates[i] * self.constantangle
        #~ return stance_rate

        
        
        
        
    def Run(self, start, target):
        start = numpy.array(start)
        target = numpy.array(target)
        dist = self.Distance(start, target)
        k = 85
            
        old_gradients = [0.0]*15
        speed = [0.0]*15
        newStance = self.parent.lastStance[:]
        while dist>2 and k>0:
            #~ print ""
            #~ for i in range(0, 15):
            stance_rate = [0.0]*15
            
            origin = numpy.array([0,0,0])
            current_elevation_plane_of_child = self.parent.lastStance[1]
            x_parent = [1,0,0] #self.parent.parent.OXYZThorax[1]
            t = vtk.vtkTransform()
            t.RotateY(current_elevation_plane_of_child)
            rotationaxis = t.TransformPoint(x_parent)
            stance_rate[0] = self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array(self.parent.parent.OXYZThorax[2])
            stance_rate[2] = -self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array([0,1,0])
            stance_rate[1] = self.SingleGradient(origin, rotationaxis, start, target)
            
            origin = numpy.array(self.parent.parent.blPos[4])
            current_elevation_plane_of_child = self.parent.lastStance[4]
            x_parent = self.parent.parent.OXYZThorax[1]
            t = vtk.vtkTransform()
            t.RotateY(current_elevation_plane_of_child)
            rotationaxis = t.TransformPoint(x_parent)
            stance_rate[3] = self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array(self.parent.parent.Right_OXYZClavicle[2])
            stance_rate[5] = -self.SingleGradient(origin, rotationaxis, start, target)
            stance_rate[4] = stance_rate[2]
            
            origin = numpy.array(self.parent.parent.blPos[6])
            current_elevation_plane_of_child = self.parent.lastStance[7]
            x_parent = self.parent.parent.Right_OXYZClavicle[1]
            t = vtk.vtkTransform()
            t.RotateY(current_elevation_plane_of_child)
            rotationaxis = t.TransformPoint(x_parent)
            stance_rate[6] = self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array(self.parent.parent.Right_OXYZScapula[2])
            stance_rate[8] = -self.SingleGradient(origin, rotationaxis, start, target)
            stance_rate[7] = stance_rate[5]
            
            origin = numpy.array(self.parent.parent.blPos[24])
            current_elevation_plane_of_child = self.parent.lastStance[10]
            x_parent = self.parent.parent.Right_OXYZScapula[1]
            t = vtk.vtkTransform()
            t.RotateY(current_elevation_plane_of_child)
            rotationaxis = t.TransformPoint(x_parent)
            stance_rate[9] = self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array(self.parent.parent.Right_OXYZHumerus[2])
            stance_rate[11] = -self.SingleGradient(origin, rotationaxis, start, target)
            stance_rate[10] = stance_rate[8]
            
            origin = numpy.array(self.parent.parent.Right_midELEM)
            current_elevation_plane_of_child = self.parent.lastStance[13]
            x_parent = self.parent.parent.Right_OXYZHumerus[1]
            t = vtk.vtkTransform()
            t.RotateY(current_elevation_plane_of_child)
            rotationaxis = t.TransformPoint(x_parent)
            stance_rate[12] = self.SingleGradient(origin, rotationaxis, start, target)
            rotationaxis = numpy.array(self.parent.parent.Right_OXYZForearm[2])
            stance_rate[14] = -self.SingleGradient(origin, rotationaxis, start, target)
            stance_rate[13] = stance_rate[11]
            
            for i in range(0,3):
                stance_rate[i] = 0
                
            newStance = self.parent.lastStance[:]
            
            for i in range(0, len(stance_rate)):
                if numpy.isnan(stance_rate[i]):
                    stance_rate[i] = 0
                    
                #~ have we gone past it?        
                if cmp(stance_rate[i], 0) != cmp(old_gradients[i], 0):
                    #~ newStance[i] = newStance[i] - speed[i] * old_gradients[i] / (stance_rate[i]-old_gradients[i])                    
                    #~ newStance[i] = newStance[i] - speed[i]
                    speed[i] = 0# speed[i] / 2
                else:
                    speed[i] = speed[i] + stance_rate[i]                 
                
                newStance[i] = newStance[i] + speed[i]
                old_gradients[i] = stance_rate[i]
                
                

                if i == 3:
                    if newStance[i] < 0:
                        newStance[i] = 0
                    elif newStance[i] > 20:
                        newStance[i] = 20
                if i == 4:
                    if newStance[i] < -15:
                        newStance[i] = -15
                    elif newStance[i] > 15:
                        newStance[i] = 15
                if i == 5:
                    if newStance[i] < -30:
                        newStance[i] = -30
                    elif newStance[i] > -12:
                        newStance[i] = -12
                for j in range(0, len(self.parent.Filters.jointAngleCollection)):
                    if self.parent.Filters.jointAngleCollection[j]['stanceID']==i:
                        if newStance[i] < self.parent.Filters.jointAngleCollection[j]['min1']:
                            newStance[i] = self.parent.Filters.jointAngleCollection[j]['min1']
                        elif newStance[i] > self.parent.Filters.jointAngleCollection[j]['max1']:
                            newStance[i] = self.parent.Filters.jointAngleCollection[j]['max1']
            #~ print newStance

            #~ self.parent.GeneratePhysique(newStance, showbones=True)
            self.parent.GeneratePhysique(newStance, showbones=False)            
            #~ self.parent.gui.Render(2)
            start = numpy.array(self.parent.parent.blPos[13])
            dist = self.Distance(self.parent.parent.blPos[13], target)
            #~ print dist 
            k = k-1
        if k == 0:
            return -1
        else: 
            return newStance
        
        
    def Buildobbtree(self):
        self.parent.kdpolydata.Update()
        self.cKdTree = vtktudoss.vtkCKdTree()
        self.cKdTree.SetInput(0, self.parent.kdpolydata)
        self.cKdTree.SetBoxsize(7.0)
        self.cKdTree.BuildKdTree()
        idlist = 0
        for i in range(0, len(self.parent.splineCollection)):
            spline = self.parent.splineCollection[i]     
            self.parent.Filters.HideSpline(spline)
        self.previoussplines = []
        
        if self.parent.splineReferences != []:
            self.parent.kdpolydata2.Update()
            self.cKdTree2 = vtktudoss.vtkCKdTree()
            self.cKdTree2.SetInput(0, self.parent.kdpolydata2)
            self.cKdTree2.SetBoxsize(7.0)
            self.cKdTree2.BuildKdTree()
            idlist = 0
            for i in range(0, len(self.parent.splineReferences)):
                spline = self.parent.splineReferences[i]                     
                self.parent.Filters.HideSpline(spline)
        self.previoussplines2 = []
        

        
    def Queryobbtree(self, point, currentelbowposition):
        previoussplineslength = len(self.previoussplines)
        for i in range(0, previoussplineslength):
            splineNr = self.previoussplines[0]
            spline = self.parent.splineCollection[splineNr]
            self.previoussplines.pop(0)
            self.parent.Filters.FilterSingleSpline(spline, False, splineNr)
       
        #Find new poses that are within range...
        laststance = None
        laststance_minimum_elbowdistance2 = 100000.0        
        self.cKdTree.QueryRegion(point)
        typearray = self.cKdTree.Getptidtypearray()
        if typearray.GetNumberOfTuples()>0:
            for i in range(0, typearray.GetNumberOfTuples()):
                splineNr = typearray.GetValue(i)
                self.previoussplines.append(splineNr)
                spline = self.parent.splineCollection[splineNr]
                if self.parent.Filters.FilterSingleSpline(spline, False, splineNr):
                    poseActor = spline['poseActor']
                    elbowposition = poseActor.GetMapper()\
                                                            .GetInput().GetPoints().GetPoint(10)
                    elbowdistance2 = vtk.vtkMath().Distance2BetweenPoints(currentelbowposition, elbowposition)
                    if laststance_minimum_elbowdistance2 > elbowdistance2:
                        laststance = spline['stance']
                        laststance_minimum_elbowdistance2 = elbowdistance2

        if self.parent.splineReferences != []:
            previoussplines2length = len(self.previoussplines2)
            for i in range(0, previoussplines2length):
                splineNr = self.previoussplines2[0]
                spline = self.parent.splineReferences[self.previoussplines2[0]]     
                self.previoussplines2.pop(0)
                self.parent.Filters.FilterSingleSpline(spline, True, splineNr)                

            self.cKdTree2.QueryRegion(point)
            typearray = self.cKdTree2.Getptidtypearray()        
            for i in range(0, typearray.GetNumberOfTuples()):
                splineNr = typearray.GetValue(i)
                self.previoussplines2.append(splineNr)
                spline = self.parent.splineReferences[splineNr]
                if self.parent.Filters.FilterSingleSpline(spline, True, splineNr):
                    poseActor = spline['poseActor']
                    elbowposition = poseActor.GetMapper()\
                                                            .GetInput().GetPoints().GetPoint(10)
                    elbowdistance2 = vtk.vtkMath().Distance2BetweenPoints(currentelbowposition, elbowposition)
                    if laststance_minimum_elbowdistance2 > elbowdistance2:
                        laststance = spline['stance']
                        laststance_minimum_elbowdistance2 = elbowdistance2

        return laststance


class MyApp(wx.App):
    def OnInit(self):
        applic = InverseKinematics(None)
        
        elbowOXYZ = [[0,0,0], [1,0,0], [0,1,0], [0,0,1]]
        GHpos = [0, 0, -10]
        elbowpos = [0,0,0]
        handpos = [10,0,0]
        handtarget = [7.07,0,7.07]
        stance = [0, 0, 90]
        applic.stance_min = [0,-10,0]
        applic.stance_max = [0,10,0]
        stance_rates = applic.DetermineGradients(handpos, handtarget, elbowOXYZ)
        print stance_rates
        return 1

if __name__ == "__main__":
    app = MyApp(0)
    app.MainLoop()
    
    
    